package test;

import chair.Chronos;
import chair.core.Diary;
import lejos.nxt.ColorLightSensor;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.TouchSensor;
import lejos.nxt.addon.ColorSensor;
import lejos.robotics.navigation.Pilot;
import lejos.robotics.navigation.TachoPilot;

/**
 * Pour un lego, c'est brique par brique qu'on va étudier les prochaines
 * compétences de Calibeta. Ici l'objectif est de suivre une ligne noire.
 * 
 * Placer le robot sur la ligne pour commencer, bien droit. Le robot franchira
 * la ligne en zig-zag.
 * 
 * @author nomhad
 * 
 */
public class PIDLight {

	/** Sous ce sueil on considère avoir affaire à du noir, au dessus du blanc */
	public static final int LUM_LIMIT = 100;

	/** de combien de degrés le robot doit tourner à chaque fois */
	public static final float TURN_ANGLE = 45;
	/**
	 * Le robot tourne soit à droite soit à gauche, change de valeur
	 * alternativement de chaque côté de la ligne
	 */
	private static boolean turn = false;

	/** A franchit récemment la ligne */
	private static boolean hasCrossedLine = false;

	/** faut driver le ptit */
	private static Pilot alex;

	/**
	 * Le robot vient de franchir la ligne, on le fait tourner d'un côté puis de
	 * l'autre
	 */
	private static void turn() {
		// Tourne alternativement dans un sens et dans l'autre
		//float angle = ((turn) ? -1 : 1) * TURN_ANGLE;
		// alex.rotate(angle);
		float steer = ((turn) ? -1 : 1) * 150;
		alex.steer(steer, TURN_ANGLE);
		turn = !turn;
		alex.forward();
	}

	/**
	 * Par convention on place le robot sur la ligne noir, dans l'axe. On va lui
	 * imprimer premier parcours.
	 */
	private static void init() {
		hasCrossedLine = true;
		// Comme le robot est sencé être aligné il faut un demi-angle
		alex.rotate(TURN_ANGLE / 2);
		alex.forward();
		turn = true;
	}

	/**
	 * @param args
	 */
	public static void main(String[] args) {
		Diary.init();
		// Le fameux recepteur tactile niché au creux du robot
		TouchSensor echine = new TouchSensor(SensorPort.S1);
		ColorLightSensor light = new ColorLightSensor(SensorPort.S3,
				ColorSensor.TYPE_COLORNONE);
		// Afin d'illuminer la ligne on va sélectionner la lumière rouge.
		// Meilleur candidat après de rapides tests.
		light.setType(ColorSensor.TYPE_COLORRED);
		alex = new TachoPilot(5, 18, Motor.C, Motor.B);
		init();
		// alex.rotate(360);
		// On fait avancer le robot
		// motorGo();
		long t = Chronos.top();
		long bakt = t;
		while (true) {
			// Dès qu'on le soulève il rend les armes
			if (!echine.isPressed()) {
				alex.stop();
				System.out.println("Bye !");
				System.exit(0);
			}

			bakt = Chronos.top();
			int lum = light.getRedComponent();
			t = Chronos.top();
			Diary.logln(" ex requete : " + (t - bakt));

			Diary.logln(" == readRed " + lum);
			// On est sur la ligne
			if (lum < LUM_LIMIT) {
				hasCrossedLine = true;
			}
			// En dehors de la ligne, change de direction seulement si on a
			// franchi la ligne
			else {
				if (hasCrossedLine) {
					hasCrossedLine = false;
					turn();
				}
			}
			// }
			// if (lum >= LUM_LIMIT) {
			// turnLeft();
			// }
		}
	}

}
